/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "bbPlotUtil.h"

int main(){
	CBetBdtLocalizer glocalizer("../res/autmap-strip.txt",0.04);
	
	bbPlotUtil plot(&glocalizer);
	Grid::TScan meas;
	meas.alloc(600);
	glocalizer.grid->getScan(0,0, -M_PI/2.0, 360, M_PI/180.0, 0.7, meas); ///Get simulated scans
	
	for(int j=0;j<meas.N;j++){
		meas.a[j]-=M_PI/2.0;
	}
	
	CEnvironmentMeasurement2D envMeas;
	
	for(int i=0;i<meas.N;i++){
		TMeasurementPoint p;
		p.x = meas.r[i]*cos(meas.a[i]);
		p.y = meas.r[i]*sin(meas.a[i]);
		envMeas.addPoint(p);
	}
	
	Grid::pose origin;
	Grid::pose testPose;
	testPose.set(0.5,0.5,M_PI/12);
	
	plot.setMeasurement(envMeas, origin, 255,0,0);
	plot.setMeasurement(envMeas, testPose, 0,255,0);
	
	envMeas.frame.setError(2.0,2.0,M_PI/4);		
	envMeas.frame.setFrame(testPose.x,testPose.y,testPose.a);
	CMatchStorage matches(550);
	glocalizer.localize(envMeas, matches);
	matches.sort();
	
	plot.setResult(matches);
	//origin.set(matches.storage[0].x,matches.storage[0].y,matches.storage[0].a);
	//plot.setMeasurement(envMeas, origin, 0,0,0);
	
	
	plot.save("test.jpg");
	
	return 0;
}